/*****
 *
 *  sketch_fontanabot_v01
 *
 *  Code to execute Fontana(esque) cuts from a Lynx6 Robotic Arm
 *
 *  2008, James Stone, http://www.manofstone.com/
 *
 *  derived from the code posted by nexisnet to this forum:
 *  http://forum.pololu.com/viewtopic.php?f=16&t=745&st=0&sk=t&sd=a&start=30
 *
 *  serial derived from  Heather Dewey-Hagborg August 15 2006  http://www.arduino.cc
 *  v0.1 - ported code from arduino serial and procesing to articulate robot 
 *  v0.2 - added debounce switch
 *
 *  NOTE: Known issue, you have to reset the micro servo board after
 *        uploading new code to the Arduino.
 *
 *****/

#include <ctype.h>
// don't change this
#define bit9600Delay 84  
#define halfBit9600Delay 42
#define bit4800Delay 188 
#define halfBit4800Delay 94 

byte tx = 3;              // pin to commuicate to the pololu board
int vibMotor = 7;         // vibration motor, to ease cutting
int sol1 = 8;             // sol #1, for clamping paper
int sol2 = 9;             // sol #2, for clamping paper
int sol3 = 10;            // sol #3, for clamping paper
int sol4 = 11;            // sol #4, for clamping paper

// debounce
int inPin = 12;           // the number of the input pin for switch
int state = LOW;      // the current state of the clamps
int reading;           // the current reading from the input pin
int previous = LOW;    // the previous reading from the input pin

// the follow variables are long's because the time, measured in miliseconds,
// will quickly become a bigger number than can be stored in an int.
long time = 0;         // the last time the output pin was toggled
long debounce = 200;   // the debounce time, increase if the output flickers
long openClamps = 5000; // time to hold button
// serial stuff
byte SWval;
int incomingByte = 0;	// for incoming serial data

// from processing
float robX = 150;  // for moving arm specically
float robY = 332;
int baseRot = 3000;
int numSegments = 3;
// test change 
float x[3];
float y[3];
float angle[3];
/*
float[] x = new float[numSegments];
float[] y = new float[numSegments];
float[] angle = new float[numSegments];
*/

float segLength = 12 * 10;
float knifeLength = 17.5 * 10;
// 17.5 cm to knife edge
float targetX, targetY;

// added for compatibility
int height = 400;
int x_length = numSegments;	// for missing array.length var

void setup() {
  // init homebrew serial port on pin tx, send only
  pinMode(tx,OUTPUT);
  digitalWrite(tx,HIGH);
  
  // setup output for motors, solenoids
  pinMode(vibMotor,OUTPUT);
  pinMode(sol1,OUTPUT);
  pinMode(sol2,OUTPUT);
  pinMode(sol3,OUTPUT);
  pinMode(sol4,OUTPUT);

  // debounce stuff
  pinMode(inPin, INPUT);

  
  // init usb serial for communication to processing @ 9600 baud
  Serial.begin(9600);
  
  // from processing
  // randomSeed(0); // same random numbers each time run
  // test code
  x[x_length - 1] = 20;     // Set base x-coordinate
  y[x_length - 1] = height;  // Set base y-coordinate
// old code
/*
  x[x.length-1] = 20;     // Set base x-coordinate
  y[x.length-1] = height;  // Set base y-coordinate
*/
}

void SWprint(int data)
{
  byte mask;
  //startbit
  digitalWrite(tx,LOW);
  delayMicroseconds(bit9600Delay);
  for (mask = 0x01; mask>0; mask <<= 1) {
    if (data & mask){ // choose bit
     digitalWrite(tx,HIGH); // send 1
    }
    else{
     digitalWrite(tx,LOW); // send 0
    }
    delayMicroseconds(bit9600Delay);
  }
  //stop bit
  digitalWrite(tx, HIGH);
  delayMicroseconds(bit9600Delay);
}

void put(int servo, int angle)
    {//servo is the servo number (typically 0-7)
    //angle is the absoltue position from 500 to 5500

       unsigned char buff[6];
       unsigned int temp;
       unsigned char pos_hi,pos_low;
       
       temp=angle&0x1f80;
       pos_hi=temp>>7;
       pos_low=angle & 0x7f;

       buff[0]=0x80;//start byte
       buff[1]=0x01;//device id
       buff[2]=0x04;//command number
       buff[3]=servo;//servo number
       buff[4]=pos_hi;//data1
       buff[5]=pos_low;//data2

       for(int i=0;i<6;i++){
          // Serial.print(buff[i],BYTE); // uses built in serial
          SWprint(buff[i]);  // uses homebrew serial
       }
    }




// servo 0, rotation of base
int angle2Servo0(float a) { // a = angle in radians    
   int center = 2830; // centered rotation
   int c90 = 4520; // 90deg to the right, clockwise
   int cc90 = 1140; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return value; 
}

// servo 1, elevation of 1st arm segment from base, shoulder
int angle2Servo1(float a) { // a = angle in radians    
   int center = 1700; // parallel with the ground plane
  // int c90 = 3337; // perpendicular to the ground plane * old value, was working, angle changed to 3k, unknown reason
   int c90 = 3000; // perpendicular to the ground plane
   int value = center;  // if below is not met, a = 0 exactly
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
  return value; 
}

// servo 2, elevation of 2nd arm segment from base, elbow
int angle2Servo2(float a) { // a = angle in radians    
   int c90 = 3400; // 90deg to the right, clockwise
   int center = c90 - (5190 - c90); // centered rotation
   int cc90 = center - (c90 - center); // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)(center - (center - c90) / 90 * (degrees(a)));
   }
  return value; 
}

// servo 2, elevation of 2nd arm segment from base, elbow
int angle2Servo3(float a) { // a = angle in radians    
   int center = 3140; // centered rotation
   int c90 = 5030; // 90deg to the right, clockwise
   int cc90 = 1250; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return value; 
}


void calcIK() {
	reachSegment(0, robX, robY);
	for(int i=1; i<numSegments; i++) {
		reachSegment(i, targetX, targetY);
	}
	for(int i=x_length-1; i>=1; i--) {
		positionSegment(i, i-1);  
	} 
/*	
	for(int i=0; i<x_length; i++) {
		segment(x[i], y[i], angle[i], (i+1)*2); 
	}
	*/
}

void moveArm() {
	put(0, baseRot); 									// move servo 0, base
	put(1, angle2Servo1(-angle[2]));					// move servo 1, shoulder
	put(2, angle2Servo2(abs(angle[2]) + angle[1]));		// move servo 2, elbow
	put(3, angle2Servo3(angle[1]));						// move servo 3, wrist
}
void randomCut() {
	int startCut = (int)random(190, 240);
	int endCut = (int)random(300, 350);
	baseRot = (int)random(2430, 3030);
	put(4, 3000); // default servo, wrist rot
	robX = 180;
	robY = startCut;
	calcIK();
	moveArm();
	delay(1000);
	robX = 210; // start cut
	calcIK();
	moveArm();
	delay(1000);
	digitalWrite(vibMotor, HIGH); // turn vibe on
	delay(1000);
	robY = endCut; // end cut
	calcIK();
	moveArm();
	delay(2000);
	digitalWrite(vibMotor, LOW); // turn vibe off
	delay(1000);
	robX = 160;
	calcIK();
	moveArm();
	delay(1000);
	robY = 332;
	robX = 150;
	calcIK();
	delay(4000);
	calcIK();
	moveArm();
	delay(500);
	baseRot = 2730; // return to center
	moveArm();
	
	robY = 250;
	robX = 150;
	calcIK();
	moveArm();
	delay(200);
	calcIK();
	moveArm();
	delay(200);
	calcIK();
	moveArm();
	delay(200);
	calcIK();
	moveArm();
	delay(200);
	calcIK();
	moveArm();

}

void positionSegment(int a, int b) {
  x[b] = x[a] + cos(angle[a]) * segLength;
  y[b] = y[a] + sin(angle[a]) * segLength; 
}

void reachSegment(int i, float xin, float yin) {
  float dx = xin - x[i];
  float dy = yin - y[i];
  if (i == 0) {
    angle[i] = 0;  
  } else {
    angle[i] = atan2(dy, dx);  
  }
  targetX = xin - cos(angle[i]) * segLength;
  targetY = yin - sin(angle[i]) * segLength;
}

void loop() {
  reading = digitalRead(inPin);
  // if we just pressed the button (i.e. the input went from LOW to HIGH),
  // and we've waited long enough since the last press to ignore any noise...  
  	if (reading == HIGH && previous == LOW && millis() - time > openClamps) {
  		if (state == HIGH) {
			digitalWrite(sol1, LOW); // open clamp
			digitalWrite(sol2, LOW); // open clamp
			digitalWrite(sol3, LOW); // open clamp
			digitalWrite(sol4, LOW); // open clamp			
			state = LOW;
      	} else {
			digitalWrite(sol1, HIGH); // open clamp
			digitalWrite(sol2, HIGH); // open clamp
			digitalWrite(sol3, HIGH); // open clamp
			digitalWrite(sol4, HIGH); // open clamp
			state = HIGH;
		}
		time = millis();
	} else if (reading == HIGH && previous == LOW && millis() - time > debounce) {
	// CHECK FOR BUTTON CONDITION
		for (int c = 0; c <= random(1,6); c++) {
			randomCut();
			delay(1000);
			calcIK();
			moveArm();
		}
		robY = 250;
		robX = 150;
		calcIK();
		moveArm();
		delay(200);
		calcIK();
		moveArm();
		delay(200);
		calcIK();
		moveArm();
		delay(200);
		calcIK();
		moveArm();
		delay(200);
		calcIK();
		moveArm();
		digitalWrite(sol1, HIGH); // open clamp
		digitalWrite(sol2, HIGH); // open clamp
		digitalWrite(sol3, HIGH); // open clamp
		digitalWrite(sol4, HIGH); // open clamp
		state = HIGH;
		time = millis();
  	}
	previous = reading;
	delay(1);
}
